package frc.circuitrunners.RobotSubsystems.Conveyor;
import edu.wpi.first.wpilibj.Victor;
import frc.circuitrunners.IO.IO;
import frc.circuitrunners.R;
import frc.circuitrunners.Scheduler;

/**
 * @author CircuitRunners
 * This package and class contains all implementation of the Fetcher code
 */
public class Conveyor extends frc.circuitrunners.RobotSubsystems.RobotSubsystem {
    
    private int counter = 0; //how many balls are currently being held by the robot 
    public int mode = 1; //0 (beast mode) just feeds every ball acquired directly to the shooter, 1 (conservative) is conservative
    private static Victor v = new Victor(2,2); //victor module for the roller motors
    
    /*initializes the subsystem*/
    public void init() {
        stage = 0;       
        setFlag((IO.getInput(IO.DIGITAL_INPUT | 1) == 1.0) ? true : false); //check if the IR sensor is active
        //check if the Victor is active
        //nothing past this point should run if setFlag() returned false
        stage++;
        }
    
     /*autonomous implementation of Fetcher subsystem*/
    public void runAutonomous() { 
        
        Scheduler.feed();
        if(!getFlag()){stage = -1;}
 
        switch(R.STAGE)
            {
            case 1: //detect player movements to control rollers, and increment the counter
                
                //detect player movements to turn on or off rollers HERE
                if(IO.getInput(IO.DIGITAL_INPUT | 1) == 0.0){
                    counter++; //increment counter when a ball passes by the IR sensor
                    setFlag(true);
                }
            case 2: //ball management
                
                //once balls are at max capacity, feed to shooter as the shooter calls for
                setFlag(true);
            case 3: //MAKE THE SHOT!
                
                if (counter < 1){
                    stage = 1;
                }
                setFlag(true);
            default: //work without the IR sensor
                manipulateRollers(100);
                setFlag(true);
            }
    }
        
    //Tele-operated implementation of Fetcher subsystem
    //STAGE 1 = IR Scan //STAGE 2 = Keeping count //STAGE 3 = Feeding //ERROR = POWER-HOG
    public void runTeleop() {
        if(!getFlag()){stage = -1;}
        Scheduler.feed();
        
        //these are fall through on purpose btw
        switch(R.STAGE)
            {
            case 1: 
                //IRSensor transmitter and receiver sits around, detects if something passes between the two systems
                //Once something passes through stage 2 begins
                
                if(IO.getInput(IO.JOY_LEFT | IO.BUTTON) == 1.0 || IO.getInput(IO.JOY_RIGHT | IO.BUTTON) == 1.0) 
                    mode = 0;
                else
                    mode = 1;
                if(IO.getInput(IO.DIGITAL_INPUT | 1) == 0.0)
                    stage++;
                
            case 2:
                //If IR equipment is above the rollers (assumed for now), update ball counter here and jump to stage 3
                counter++;
                stage++;
                
            //to understand the logic of this stage you will need to wrap your head around the iteration during a game as 
            //balls are picked up and shot
            case 3:
                limit(mode);
                if(counter > mode && counter <= 3)
                {
                    //convey ball to shooter
                //Shooter.feed(): //shooter will update its ball count here
                //james-liu: limit switch?
                counter--;
                
                if(!getFlag()){
                    manipulateRollers(100);
                    setFlag(true);
                    }
                }
                
                else if(counter > 3)
                {
                manipulateRollers(0);
                setFlag(false);
                }
                
                else if(IO.getInput(IO.DIGITAL_INPUT) == 1.0 && counter < 1)
                {
                stage = 1;
                }
                
            default:
                //FAIL???
                manipulateRollers(100);
                setFlag(false);
            }     
    } 
    
    /*a global method to control rollers*/
    public static void manipulateRollers(int speed){
        v.set(speed);
    }
    
    /*disabling the robot should stop the roller and IR sensor*/
    public void disable(){
        v.set(0);
        Scheduler.feed();
    }
    
    /**
     * It is imperative that the roller is stopped during autoaim
     */
    public void autoAim() {
        disable();
        //re-initialize after auto-aiming is complete
    }
    
    //limiter method for the fetcher mode
    public void limit(int mode){
        if (mode != 1 || mode != 0)
            mode = 1;
    }
}
